// Coarse collision detection system

#pragma once

#include "contacts.h"

namespace physics {

	//////////////////////////////////
	//		BoundingSphere
	//////////////////////////////////
	// This object contains information
	// which represents a spherical
	// bounding volume
	struct BoundingSphere
	{
		Vector3 center;
		real radius;

	public:
		// Constructors
		BoundingSphere(const Vector3 &center, real radius);
		BoundingSphere(const BoundingSphere &one, const BoundingSphere &two);

		// returns true if the bounding spheres overlap
		bool overlaps(const BoundingSphere *other) const;

		// returns how much the bounding sphere would need
		// to grow to encapsulate the given bounding sphere.
		// This method is not ideal, since the best implementation
		// would compare the growth in surface area, but this
		// is sufficient for our needs
		real getGrowth(const BoundingSphere &other) const;

		// returns the volume of the bounding sphere
		real getSize() const
		{
			return ((real)1.333333) * R_PI * radius * radius * radius; // 4/3*Pi*R^3
		}
	};

	struct PotentialContact
	{
		// contains a pair of rigidbodies that COULD
		// be in contact with one-another
		RigidBody* body[2];
	};

	// Base class for bounding volume hierarchies
	// Uses a binary tree to store bounding volumes
	template<class BoundingVolumeClass>
	class BVHNode
	{
	public:
		// holds the children of this node
		BVHNode *children[2];

		// Holds the bounding volume which envelops all
		// children (and hence all descendents) of this node
		BoundingVolumeClass volume;

		// holds the RigidBody at this node
		RigidBody *body;

		// parent node
		BVHNode* parent;

		// creates a new node hierarchy
		BVHNode(BVHNode *parent, const BoundingVolumeClass &volume, RigidBody* body = NULL)
			: parent(parent), volume(volume), body(body)
		{
			children[0] = children[1] = NULL;
		}

		// checks if this node is a leaf
		bool isLeaf() const
		{
			// if I don't have a rigid body, return true
			return (body != NULL);
		}

		// checks potential contacts throughout the hierarchy
		unsigned getPotentialContacts(PotentialContact* contacts, unsigned limit) const;

		// inserts the given rigid body with teh given bounding volume into
		// the hierarchy. Could involve the creation of further nodes.
		void insert(RigidBody* body, const BoundingVolumeClass &volume);

		// deletes this node
		~BVHNode();

	protected:
		// checks for overlapping between nodes.
		// all bounding volumes need to have an overlaps()
		// method implemented for this to work
		bool overlaps(const BVHNode<BoundingVolumeClass> *other) const;

		// checks potential contacts between this node and another node
		unsigned getPotentialContactsWith(const BVHNode<BoundingVolumeClass> *other, PotentialContact* contacts, unsigned limit) const;

		// recalculates the bounding volume if its children have
		// changed
		void recalculateBoundingVolume(bool recurse = true);

	};

	// need to define everything in this header
	template<class BoundingVolumeClass>
	bool BVHNode<BoundingVolumeClass>::overlaps( const BVHNode<BoundingVolumeClass> *other) const
	{
		return volume->overlaps(other->volume);
	}

	template<class BoundingVolumeClass>
	void BVHNode<BoundingVolumeClass>::insert(RigidBody *newBody, const BoundingVolumeClass &newVolume)
	{
		// if we are a leaf, then create new children
		if (isLeaf())
		{
			// first child is a copy of us
			children[0] = new BVHNode<BoundingVolumeClass>(this, volume, body);

			// second child holds the new guy
			children[1] = new BVHNode<BoundingVolumeClass>(this, newVolume, newBody);

			// we are no longer a leaf
			this->body = NULL;

			// recalculate the bounding volume since we've 'changed'
			recalculateBoundingVolume();
		}
		else
		{
			// otherwise we need to figure out which child
			// keeps the body by figuring out who would
			// grow the least
			if (children[0]->volume.getGrowth(newVolume) < children[1]->volume.getGrowth(newVolume))
			{
				children[0]->insert(newBody, newVolume);
			}
			else
			{
				children[1]->insert(newBody, newVolume);
			}
		}
	}

	template<class BoundingVolumeClass>
	BVHNode<BoundingVolumeClass>::~BVHNode()
	{
		// if we have a parent, we need to process our sibling
		// since it will be destroyed and merged into our parent
		// as a result of our own deletion
		if (parent)
		{
			// find our sibling
			BVH<BoundingVolumeClass> *sibling;

			if (parent->children[0] == this)
				sibling = parent->children[1];
			else
				sibling = parent->children[0];

			// write the data to the parent
			parent->volume = sibling->volume;
			parent->body = sibling->body;
			parent->children[0] = sibling->children[0];
			parent->children[1] = sibling->children[1];

			// delete the sibling
			sibling->parent = NULL;
			sibling->body = NULL;
			sibling->children[0] = NULL;
			sibling->children[1] = NULL;
			delete sibling;

			// recalculate the parent's volume since its changed
			parent->recalculateBoundingVolume();
		}

		// clear our children's parent data and delete them
		if (children[0]) {
			children[0]->parent = NULL;
			delete children[0];
		}

		if (children[1]) {
			children[1]->parent = NULL;
			delete children[1];
		}
	}

	template<class BoundingVolumeClass>
	void BVHNode<BoundingVolumeClass>::recalculateBoundingVolume(bool recurse)
	{
		if (isLeaf())
			return; // no need to process leaves, which have no volumes

		volume = BoundingVolumeClass(children[0]->volume, children[1]->volume);

		// recurse up the tree to inform our parents that we've
		// changed and they need to be processed too
		if (parent)
			parent->recalculateBoundingVolume(true);
	}

	template<class BoundingVolumeClass>
	unsigned BVHNode<BoundingVolumeClass>::getPotentialContacts(PotentialContact* contacts, unsigned limit) const
	{
		// if we're a leaf, or we don't have 'room' for contacts, exit
		if (isLeaf() || limit == 0)
			return 0;

		// get potential contacts between our children
		return children[0]->getPotentialContactsWith(children[1], contacts, limit);
	}

	template<class BoundingVolumeClass>
	unsigned BVHNode<BoundingVolumeClass>::getPotentialContactsWith(const BVHNode<BoundingVolumeClass> *other, PotentialContact *contacts, unsigned limit) const
	{
		// if we don't overlap, or have no 'room' for contacts, exit
		if (!overlaps(other) || limit == 0)
			return 0;

		// if both objects are leaf nodes, we have a potential contact.
		// Note POTENTIAL contact. The bodies could still be far apart
		// but our bounding volumes are as small at possible at this point
		// so its worth checking out.
		if (isLeaf() && other->isLeaf())
		{
			contacts->body[0] = body;
			contacts->body[1] = other->body;
			return 1;
		}

		// determine which node to check. If one is a leaf
		// we check the other. If both are branches, we 
		// check the one with a larger size.

		if (other->isLeaf() || (!isLeaf() && volume->getSize() >= other->volume->getSize()))
		{
			// recurse into self
			unsigned count = children[0]->getPotentialContactsWith(other, contacts, limit);

			// check if we can do the other side, too
			if (limit > count)
			{
				return count + children[1]->getPotentialContactsWith(other, contacts+count, limit-count);
			}
			else
			{
				return count;
			}
		}
		else
		{
			// recurse into the other one
			unsigned count = getPotentialContactsWith(other->children[0], contacts, limit);

			// check if we have enough limit left
			if (limit > count)
			{
				return count + getPotentialContactsWith(other->children[1], contacts+count, limit-count);
			}
			else
			{
				return count;
			}
		}
	}
}